package robot;

import javax.vecmath.Point2d;
import javax.vecmath.Vector3d;

import pathAlgorithms.PathAlgorithm;
import program.Main;
import program.MovingObjects;

/**
 * Robots using A*
 * @date 15 maj 2012
 * @version 1.0
 */
public class AStarRobot extends Robot{
	private PathAlgorithm pathfinder;
	private Point2d[] path;
	private Point2d goal;

	public AStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo) {
		super(position, name, mo);
		this.pathfinder = pathfinder;
		this.goal = goal;
	}
	
	public AStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo, int sensorRange) {
		super(position, name, mo,sensorRange);
		this.pathfinder = pathfinder;
		this.goal = goal;
	}
	
	public AStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo, int sensorRange, boolean assumeConcave, int concaveRange) {
		super(position, name, mo, sensorRange,assumeConcave,concaveRange);
		this.pathfinder = pathfinder;
		this.goal = goal;
		roboLogger.path.add(getCurrentPosition());
		
	}

	@Override
	public void performBehavior() {
		
		if(!isFinished()) {
			
	        if(this.path == null) {
	            this.updateRoboMap();
	            this.path = this.pathfinder.getPath(this.roboMap, getCurrentPosition(), this.goal, getLifeTime());
	            if(path != null) {
	            	roboLogger.pathsCalculated++;
	            }
	            
	            setTranslationalVelocity(1);
	        }
	    
	        if(sensors.getMeasurement(0) < 0.4 || 
	 	       sensors.getMeasurement((int)(0.125*sensors.getNumSensors())) < 0.4 ||
	 	       sensors.getMeasurement((int)(0.875*sensors.getNumSensors())) < 0.4) {
	            updateRoboMap();
	            this.path = pathfinder.getPath(roboMap, getCurrentPosition(), goal, getLifeTime());
	            if(path != null) {
	            	roboLogger.pathsCalculated++;
	            }
	               
	            this.setIndex(0);
	        }
	    
	        if(this.path == null) {
	                    setTranslationalVelocity(0);
	                    this.setFinished(true);
	        }
		    
	        if(this.path != null) {
	        
			    rotateTowardsNextGoal(this.path[getIndex()]);
			    
			    if (this.getCurrentPosition().distance(this.path[getIndex()]) < ACCEPT_DISTANCE) {
			    	 	
			    	roboLogger.path.add(path[getIndex()]);
		    		setIndex(getIndex()+1);
		            if(getIndex() == path.length) {
		                    setTranslationalVelocity(0);
		                    setFinished(true);
		                    return;
		            }
		            rotateTowardsNextGoal(path[getIndex()]);
			    }
	        }
		}
		
		if(this.path == null) {
			
			if(!firstFinish) {
				finish("No path to goal... :'(");
			}
			if(rotate == 5) { 
				rotate = 0;
				rotateDir *= -1;
			}
			rotateY(rotateDir);
			rotate++;
			return;
		}
		
		else if(this.getCurrentPosition().distance(goal) < ACCEPT_DISTANCE) {
			
			if(!firstFinish) {
				finish("Reached goal! :)");
			}
			rotateY(0.1f);
			return;
		}

	}
	
	private void finish(String message) {
		roboLogger.distanceTraveled = getOdometer();
		System.out.println(message);
		System.out.println("------------------");
		System.out.println("------------------");
		printStatistics();
		firstFinish = true;
		setTranslationalVelocity(0);
		Main.finishRun(roboLogger, "AStarRobot "+getName()+", "+message);

	}
	
	private boolean firstFinish = false;
	private int rotate = 0;
	private float rotateDir = 0.1f;

}
